#include <stdexcept>

// vio
#include "vio/front_end/camera.h"
#include "vio/front_end/feature.h"
#include "vio/front_end/frame.h"
#include "vio/front_end/point.h"
#include "vio/vio_common.h"

// vio utils
#include "vio/tools/math_utils.h"

namespace vio {

int Point::point_counter_ = 0;

Point::Point(const Eigen::Vector3d& pos)
    : id_(point_counter_++),
      pos_(pos),
      normal_set_(false),
      n_obs_(0),
      last_published_ts_(0),
      last_projected_kf_id_(-1),
      type_(TYPE_UNKNOWNM),
      n_failed_reproj_(0),
      n_succeeded_reproj_(0),
      last_structure_optim_ts_(0) {}

Point::Point(const Eigen::Vector3d& pos, Feature::Ptr ftr)
    : id_(point_counter_++),
      pos_(pos),
      normal_set_(false),
      n_obs_(0),
      last_published_ts_(0),
      last_projected_kf_id_(-1),
      type_(TYPE_UNKNOWNM),
      n_failed_reproj_(0),
      n_succeeded_reproj_(0),
      last_structure_optim_ts_(0) {
  obs_.push_back(ftr);
}

void Point::AddFrameRef(Feature::Ptr ftr) {
  obs_.push_back(ftr);
  ++n_obs_;
}

// find feature in a frame for this point
Feature::Ptr Point::FindFrameRef(Frame::Ptr frame) {
  for (auto it = obs_.begin(); it != obs_.end(); it++) {
    if ((*it)->frame_ptr.lock() == frame) {
      return *it;
    }
  }
  return nullptr;
}

// delete frame's feature for this point
bool Point::DeleteFrameRef(Frame::Ptr frame) {
  for (auto it = obs_.begin(); it != obs_.end(); it++) {
    if ((*it)->frame_ptr.lock() == frame) {
      obs_.erase(it);
      return true;
    }
  }
  return false;
}

void Point::InitNormal() {
  assert(!obs_.empty());
  const Feature::Ptr ftr = obs_.back();
  auto frame_ptr = ftr->frame_ptr.lock();
  assert(frame_ptr != nullptr);
  // camera frame bearing -> T_w_f -> world frame normal, pointer to camera
  normal_ = frame_ptr->T_f_w_.rotationMatrix().transpose() * (-ftr->bearing);
  const double dist = (pos_ - frame_ptr->pos()).norm() + 1.0e-8;
  normal_information_ =
      Eigen::DiagonalMatrix<double, 3, 3>(std::pow(20 / dist, 2), 1.0, 1.0);
  normal_set_ = true;
}

bool Point::GetCloseViewObs(const Eigen::Vector3d& frame_pos,
                            Feature::Ptr& ftr) const {
  // TODO: get frame with same point of view and same pyramid level.
  Eigen::Vector3d obs_dir(frame_pos - pos_);
  obs_dir.normalize();
  auto min_it = obs_.begin();
  double min_cos_angle = 0;
  // find min cos angle
  for (auto it = obs_.begin(); it != obs_.end(); it++) {
    if (auto f_ptr = (*it)->frame_ptr.lock()) {
      Eigen::Vector3d dir((f_ptr->pos() - pos_));
      dir.normalize();
      const double cos_angle = obs_dir.dot(dir);
      if (cos_angle > min_cos_angle) {
        min_cos_angle = cos_angle;
        min_it = it;
      }
    }
  }
  ftr = *min_it;
  // assume that observations larger than 60 deg are useless
  if (min_cos_angle < 0.5) {
    return false;
  }
  return true;
}

void Point::Optimize(const size_t n_iter) {
  Eigen::Vector3d old_point = pos_;
  double chi2 = 0.0;
  Eigen::Matrix3d A;
  Eigen::Vector3d b;

  for (size_t i = 0; i < n_iter; i++) {
    A.setZero();
    b.setZero();
    double new_chi2 = 0.0;

    // compute residuals
    for (auto it = obs_.begin(); it != obs_.begin(); it++) {
      Matrix23d J;
      if (auto f_ptr = (*it)->frame_ptr.lock()) {
        // point in this frame camera frame
        // reproject landmark point to each frame and calculate reproject error
        const Eigen::Vector3d p_cam(f_ptr->T_f_w_ * pos_);
        Point::jacobian_xyz2uv(p_cam, f_ptr->T_f_w_.rotationMatrix(), J);
        const Eigen::Vector2d proj_error(math_utils::project2d((*it)->bearing) -
                                         math_utils::project2d(p_cam));
        new_chi2 += proj_error.squaredNorm();
        // gauss-newton solver
        A.noalias() += J.transpose() * J;
        b.noalias() -= J.transpose() * proj_error;
      }
    }
    // solve linear system
    const Eigen::Vector3d delta_pos(A.ldlt().solve(b));
    // check if error increased
    if ((i > 0 && new_chi2 > chi2) && bool(std::isnan(double(delta_pos[0])))) {
      pos_ = old_point;  // roll-back
      break;
    }
    // update the model(point's pos)
    Eigen::Vector3d new_point = pos_ + delta_pos;
    old_point = pos_;
    chi2 = new_chi2;
    // stop when converged
    if (delta_pos.norm() < 1.0e-6) {
      break;
    }
  }
}
}  // namespace vio